/********************************************************************************
*  Project   		: Neurologist
*  File Name  		: Neurologist.cpp        
*  Contributors 	: 599 Code Monkeys
*  Creation Date 	: Jan. 17, 2009
*  Revision History	: Source code & revision history maintained at 10.101.29.110 
*  File Description	: Implementation of Robodox 2009 FRC robot, the Neurologist.
* 					  Contains drive and manipulator control functions along with
* 					  autonomous camera tracking
*/                         
/*----------------------------------------------------------------------------*/
/*        Copyright (c) FIRST 2008.  All Rights Reserved.                     */
/*  Open Source Software - may be modified and shared by FRC teams. The code  */
/*  must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/


#include "Neurologist.h"

// To locally enable debug printing: set VisionDemo_debugFlag to a 1, to disable set to 0
int Neurologist_debugFlag = 0;
#define DPRINTF if(Neurologist_debugFlag)dprintf

#define PI (3.14159265358979)

#define UPDATE_TIME_SECONDS (0.05)
// for 160x120, 50 pixels = .38 %
#define MIN_PARTICLE_TO_IMAGE_PERCENT 0.25		// target is too small
#define MAX_PARTICLE_TO_IMAGE_PERCENT 20.0		// target is too close


#define CAL_SWITCH_PORT (1)


/** Simple test to see if the color is taking up too much of the image */
int tooClose(ParticleAnalysisReport* par) {
	if (par->particleToImagePercent > MAX_PARTICLE_TO_IMAGE_PERCENT)
		return 1;
	return 0;
}

/** Simple test to see if the color is large enough */
int bigEnough(ParticleAnalysisReport* par) {
	if (par->particleToImagePercent < MIN_PARTICLE_TO_IMAGE_PERCENT)
		return 0;
	return 1;
}


static void Update(Neurologist *neuro){
	float headingLast = 0;
	float dist_l = 0.;
	float dist_r = 0.;
	float dist_lref = 0.;
	float dist_rref = 0.;
	float dist_l_last = 0.;
	float dist_r_last = 0.;
	float dist_lref_last = 0;
	float dist_rref_last = 0;
	float velocityXlast = 0;
	float velocityYlast = 0;
		
	while(1){
		//now save the last values
		dist_l_last = dist_l;
		dist_r_last = dist_r;	
		dist_lref_last = dist_lref;
		dist_rref_last = dist_rref;	
		
		dist_l =  neuro->leftGearbox->GetDistance();
		dist_r = neuro->rightGearbox->GetDistance();
		dist_rref = neuro->rightRef->GetDistance();
		dist_lref = neuro->leftRef->GetDistance();
		
		neuro->v_l = (dist_l - dist_l_last)/(UPDATE_TIME_SECONDS); 
		neuro->v_r = (dist_r - dist_r_last)/(UPDATE_TIME_SECONDS); 
		neuro->v_lref = (dist_lref-dist_lref_last)/(UPDATE_TIME_SECONDS);
		neuro->v_rref = (dist_rref-dist_rref_last)/(UPDATE_TIME_SECONDS);
			
		//Compute motor speed for traction controler
		neuro->l_rate_rps = neuro->v_l/neuro->w_radius;
		neuro->r_rate_rps = neuro->v_r/neuro->w_radius;
				
		//Compute average values for use within other computations.
		neuro->Mr_Robauto->v_robot = (neuro->v_lref + neuro->v_rref)/2;
		neuro->v_avg = (neuro->v_l + neuro->v_r) /2;
				
		neuro->accelerationX = neuro->a_x->GetAcceleration();
		neuro->accelerationY = neuro->a_y->GetAcceleration();
				
		// needs to be fixed so constant acceleration != 0 velocity
		neuro->velocityX = velocityXlast + (neuro->accelerationX*UPDATE_TIME_SECONDS);
		neuro->velocityY = velocityYlast + (neuro->accelerationY*UPDATE_TIME_SECONDS);		
		velocityXlast = neuro->velocityX;
		velocityYlast = neuro->velocityY;
			
		// TODO: Create own Gyro Rate function
		headingLast = neuro->heading;
		neuro->heading = neuro->g1->GetAngle();
		neuro->headingRate = (neuro->heading - headingLast)/(UPDATE_TIME_SECONDS);
			
		Wait(UPDATE_TIME_SECONDS);
	}
}


	
	/**
	 * Constructor for this robot subclass.
	 * Create an instance of a RobotDrive with left and right motors plugged into PWM
	 * ports 1 and 2 on the first digital module. The two servos are PWM ports 3 and 4.
	 */
	Neurologist::Neurologist(void):
		updateTask((char *)"Update", (FUNCPTR)Update, 75)
	{
		GetWatchdog().SetExpiration(100);
		
		cal_switch_on = false;
		
		ds = DriverStation::GetInstance();
		driveStick = new Joystick(1);			// create the joysticks
		liftStick = new Joystick(2);
		
		time = new RobodoxTimer;
		
		maxWheelRate = (12/Ke)/gearRatio;		//Ke is in volts/(radian/second)
		
		horizontalServo = new Servo(4,10); 		// create horizontal servo   (changed from slot 6)
		verticalServo = new Servo(4,9);			// create vertical servo
		
		leftMotor = new RobodoxJaguar(4, 1, false);
		rightMotor = new RobodoxJaguar(4, 2, true);
		liftR = new Relay(4,1);
		liftL = new Relay(4,2);
		rollerL = new Victor(4,3);
		rollerR = new Victor(4,4);
		g1 = new Gyro(1);
		g1->SetSensitivity(.007); // measured in volts/degree/second
		
		limitFile = fopen("robotData.txt", "r");
		fread(robotData, 4, sizeof(float), limitFile);
		fclose(limitFile);

		leftCmd_limit_pos = robotData->leftCmd_limit_pos;
		leftCmd_limit_neg = robotData->leftCmd_limit_neg;
		rightCmd_limit_pos = robotData->rightCmd_limit_pos;
		rightCmd_limit_neg = robotData->rightCmd_limit_neg;
		
		leftGearbox = new Encoder(6,1,6,2);	  //changed from leftfront
		rightGearbox = new Encoder (6,3,6,4);  //changed from rightfront
		rightRef = new Encoder (6,9,6,10);
		leftRef = new Encoder (6,11,6,12);
		
		leftRef->SetDistancePerPulse((2.0*ref_radius*PI)/1000);
		rightRef->SetDistancePerPulse((2.0*ref_radius*PI)/1000);
		leftGearbox->SetDistancePerPulse((2.0*w_radius*PI)/1000);
		rightGearbox->SetDistancePerPulse((2.0*w_radius*PI)/1000);
				
		rightRef->SetReverseDirection(true);
		rightGearbox->SetReverseDirection(true);
		
		leftRef->Start();
		rightRef->Start();
		leftGearbox->Start();
		rightGearbox->Start();
		
		v_battery = 12;
		
		v_l = 0;   //  left side wheel speed inches per second
		v_r = 0;  //  right side wheel speed ips
		v_rref = 0.;// right reference wheel speed ips
		v_lref = 0.;// left reference wheel speed ips
		l_rate_rps = 0.;  //left wheel speed rps      
		r_rate_rps = 0.;  //right wheel speed rps
		dist_l = 0.;
		dist_r = 0.;

		leftCmd_limit_pos = 0.;
		rightCmd_limit_pos = 0.;
		leftCmd_limit_neg = 0.;
		rightCmd_limit_neg = 0.;
		
		Mr_Robauto->v_robot = 0.;                                        
		v_avg = 0.;
		
		a_x = new Accelerometer(2,1); //possibly remove later 
		a_y = new Accelerometer(2,2);
		a_x->SetSensitivity(300); // millivolts/g
		a_y->SetSensitivity(300); // millivolts/g
		
		toplimitswitch = new DigitalInput(4,1);
		bottomlimitswitch = new DigitalInput(4,2);
		
		autoswitch1 = new DigitalInput(4,4);
		autoswitch2 = new DigitalInput(4,5);
		//allianceswitch = new DigitalInput(4,6);
		
		deadzone = 0.039;
		slope = 1.04;
		intercept = 0.041; 
		
		
		Mr_Robauto = new NeuroAuto(leftMotor, rightMotor, leftGearbox, rightGearbox, rightRef, leftRef, time, g1);  //YAP: took out left/right rear
		
		servoDeadband = 0.01;					// move if > this amount 
		framesPerSecond = 15;					// number of camera frames to get per second
		sinStart = 0.0;							// control where to start the sine wave for pan
		memset(&par1,0,sizeof(ParticleAnalysisReport));			// initialize particle analysis report
		memset(&par2,0,sizeof(ParticleAnalysisReport));			// initialize particle analysis report

		/* image data for tracking - override default parameters if needed */
		/* recommend making PINK the first color because GREEN is more 
		 * subsceptible to hue variations due to lighting type so may
		 * result in false positives */
		// PINK				
		sprintf (td1.name, "PINK");
		td1.hue.minValue = 220;   
		td1.hue.maxValue = 255;  
		td1.saturation.minValue = 75;   
		td1.saturation.maxValue = 255;      
		td1.luminance.minValue = 85;  
		td1.luminance.maxValue = 255;
		// GREEN
		sprintf (td2.name, "GREEN");
		td2.hue.minValue = 55;   
		td2.hue.maxValue = 125;  
		td2.saturation.minValue = 58;   
		td2.saturation.maxValue = 255;    
		td2.luminance.minValue = 92;  
		td2.luminance.maxValue = 255;
				
		/* set up debug output: 
		 * DEBUG_OFF, DEBUG_MOSTLY_OFF, DEBUG_SCREEN_ONLY, DEBUG_FILE_ONLY, DEBUG_SCREEN_AND_FILE 
		 */
		SetDebugFlag(DEBUG_SCREEN_ONLY);
				
		/* start the CameraTask	 */
		if (StartCameraTask(framesPerSecond, 0, k320x240, ROT_0) == -1) {
			DPRINTF(LOG_ERROR,"Failed to spawn camera task; exiting. Error code %s", GetVisionErrorText(GetLastVisionError()) );
		}
		/* allow writing to vxWorks target */
		Priv_SetWriteFileAllowed(1);   		

		/* stop the watchdog if debugging  */
		GetWatchdog().SetExpiration(0.5);
		GetWatchdog().SetEnabled(false);
		
		
		//updateTask.Start((INT32)this);
	}
	
	Neurologist::~Neurologist(){
		delete driveStick;			// joystick 1 (drive)
		delete liftStick;			// joystick 2 (roller)
		delete ds;				// driver station object
		delete horizontalServo;  		// first servo object
		delete verticalServo;			// second servo object
			
		delete leftMotor;
		delete rightMotor;
		delete liftR;
		delete liftL; 
		delete rollerL;
		delete rollerR;
			
		delete Mr_Robauto;
		delete toplimitswitch;
		delete bottomlimitswitch;
			
		delete autoswitch1;
		delete autoswitch2;
		//delete allianceswitch;
	
		delete time;
		
		delete g1;
	    delete a_x;
	    delete a_y;
			
		delete leftGearbox;
		delete rightGearbox;
		delete rightRef;
		delete leftRef;
	}
	

	/**
	 * Set servo positions (0.0 to 1.0) translated from normalized values (-1.0 to 1.0). 
	 * 
	 * @param normalizedHorizontal Pan Position from -1.0 to 1.0.
	 * @param normalizedVertical Tilt Position from -1.0 to 1.0.
	 */
	void Neurologist::setServoPositions(float normalizedHorizontal, float normalizedVertical)	{

		float servoH = NormalizeToRange(normalizedHorizontal);
		float servoV = NormalizeToRange(normalizedVertical);
				
		float currentH = horizontalServo->Get();		
		float currentV = verticalServo->Get();
				
		/* make sure the movement isn't too small */
		if ( fabs(servoH - currentH) > servoDeadband ) {
			horizontalServo->Set( servoH );
			/* save new normalized horizontal position */
			horizontalPosition = RangeToNormalized(servoH, 1);
		}
		if ( fabs(servoV - currentV) > servoDeadband ) {
			verticalServo->Set( servoV );
			verticalPosition = RangeToNormalized(servoV, 1);
		}
	}

	/**
	* Adjust servo positions (0.0 to 1.0) translated from normalized values (-1.0 to 1.0). 
	* 
	* @param normalizedHorizontal Pan adjustment from -1.0 to 1.0.
	* @param normalizedVertical Tilt adjustment from -1.0 to 1.0.
	*/
	void Neurologist::adjustServoPositions(float normDeltaHorizontal, float normDeltaVertical)	{
							
		/* adjust for the fact that servo overshoots based on image input */
		normDeltaHorizontal /= 8.0;
		normDeltaVertical /= 4.0;
			
		/* compute horizontal goal */
		float currentH = horizontalServo->Get();
		float normCurrentH = RangeToNormalized(currentH, 1);
		float normDestH = normCurrentH + normDeltaHorizontal;	
		/* narrow range keep servo from going too far */
		if (normDestH > 1.0) 
			normDestH = 1.0;
		if (normDestH < -1.0) 
			normDestH = -1.0;			
		/* convert input to servo range */
		float servoH = NormalizeToRange(normDestH);

		/* compute vertical goal */
		float currentV = verticalServo->Get();
		float normCurrentV = RangeToNormalized(currentV, 1);
		float normDestV = normCurrentV + normDeltaVertical;	
		if (normDestV > 1.0) 
			normDestV = 1.0;
		if (normDestV < -1.0) 
			normDestV = -1.0;
		/* convert input to servo range */
		float servoV = NormalizeToRange(normDestV, 0.2, 0.8);

		/* make sure the movement isn't too small */
		if ( fabs(currentH-servoH) > servoDeadband ) {
			horizontalServo->Set( servoH );		
			/* save new normalized horizontal position */
			horizontalPosition = RangeToNormalized(servoH, 1);
		}			
		if ( fabs(currentV-servoV) > servoDeadband ) {
			verticalServo->Set( servoV );
			verticalPosition = RangeToNormalized(servoV, 1);
		}
	}		
	
	// JDM
	// Turn robot so that camera is straight and drive towards target
	// Left motor works fine, right chatters back and forth
	// TODO: Fix
	void Neurologist::CameraDrive(float servo) { 
		float left_motor, right_motor;
		
		left_motor = leftMotor->Get();
		right_motor = rightMotor->Get();
		
		left_motor += servo;
		right_motor -= servo;
		
		//ShowActivity("Left = %f\t Right = %f", left_motor, right_motor);  //YULIYA: CHECK, IMPORTANT!!!!
		
		leftMotor->Set(left_motor);
		rightMotor->Set(right_motor);
	}
	
	
	void Neurologist::Autonomous(void) {
		DPRINTF(LOG_DEBUG, "Autonomous");				
				
		DPRINTF(LOG_DEBUG, "SERVO - looking for COLOR %s ABOVE %s", td2.name, td1.name);
				
		// initialize position and destination variables
		// position settings range from -1 to 1
		// setServoPositions is a wrapper that handles the conversion to range for servo 
		horizontalDestination = 0.0;		// final destination range -1.0 to +1.0
		verticalDestination = 0.0;
				
		// initialize pan variables
		// incremental tasking toward dest (-1.0 to 1.0)
		float incrementH, incrementV;
		// pan needs a 1-up number for each call
		int panIncrement = 0;							
				
		// current position range -1.0 to +1.0
		horizontalPosition = RangeToNormalized(horizontalServo->Get(),1);	
		verticalPosition = RangeToNormalized(verticalServo->Get(),1);			
				
		// set servos to start at center position
		setServoPositions(horizontalDestination, verticalDestination);

		// for controlling loop execution time 
		float loopTime = 0.1;		
		//float loopTime = 0.05;											
		double currentTime = GetTime();
		double lastTime = currentTime;
												
		// search variables 
		bool foundColor = 0; 
		double savedImageTimestamp = 0.0;
		bool staleImage = false; 
		
		SecondColorPosition colorPosition;
		
		/*if(allianceswitch->Get() == 1)	 //green on top
			colorPosition = ABOVE;
		
		else 	//pink on top
			colorPosition = BELOW;
		*/
		if(ds->GetAlliance()==DriverStation::kRed)  //red alliance = pink above green
			colorPosition = ABOVE;
		else if(ds->GetAlliance()==DriverStation::kBlue)  //blue alliance = green above pink
			colorPosition = BELOW;
		else
			colorPosition = RIGHT;  //random
			
		while( IsAutonomous() )	{
			//GetWatchdog().Feed();		// turn watchdog off while debugging
			
		/*	if(autoswitch1a->Get()== 1 && autoswitch2a->Get() == 1)   if using 2 three position switches
				Mr_Robauto->AroundDrive(1);
			else if(autoswitch1a->Get() == 1 && autoswitch2b->Get()==1)
				Mr_Robauto->AroundDrive(2);
			else if(autoswitch1a->Get()==1 && autoswitch2a->Get()!=1 && autoswitch2b->Get()!=1)
				Mr_Robauto->AroundDrive(3);
			else if(autoswitch1b->Get()==1 && autoswitch2a->Get()==1)
				Mr_Robauto->CornerCircling();
			else if(autoswitch1b->Get==1 && autoswitch2b->Get()==1)
				Mr_Robauto->CenterCircling();
			else if(autoswitch1a->Get()!=1 && autoswitch1b->Get()!=1)
				Mr_Robauto->RandomDrive();
		*/  
			
			if(autoswitch1->Get()==1 && autoswitch2->Get()==1)
				Mr_Robauto->RandomDrive();
			else if(autoswitch1->Get()==1 && autoswitch2->Get()==0)
				Mr_Robauto->CornerCircling();
			else if(autoswitch1->Get()==0 && autoswitch2->Get()==0)
				Mr_Robauto->CenterCircling();
			else
				Mr_Robauto->AroundDrive(ds->GetLocation());
			
			
			// calculate gimbal position based on colors found 
			if ( FindTwoColors(td1, td2, colorPosition, &par1, &par2) ){
				//PrintReport(&par2);
				foundColor = true;
				// reset pan		
				panIncrement = 0;  		
				if (par1.imageTimestamp == savedImageTimestamp) {
					// This image has been processed already, 
					// so don't do anything for this loop 
					staleImage = true;
					DPRINTF(LOG_DEBUG, "STALE IMAGE");
					
				} 
				else {
					// The target was recognized
					// save the timestamp
					staleImage = false;
					savedImageTimestamp = par1.imageTimestamp;	
					DPRINTF(LOG_DEBUG,"image timetamp: %lf", savedImageTimestamp);

					// Here is where your game-specific code goes
					// when you recognize the target
					
					// get center of target 
					// Average the color two particles to get center x & y of combined target
					horizontalDestination = (par1.center_mass_x_normalized + par2.center_mass_x_normalized) / 2;	
					verticalDestination = (par1.center_mass_y_normalized + par2.center_mass_y_normalized) / 2;							
				}
			} 
			else   // need to pan 
				foundColor = false;
			 
								
			if(foundColor && !staleImage) {	
				/* Move the servo a bit each loop toward the destination.
				 * Alternative ways to task servos are to move immediately vs.
				 * incrementally toward the final destination. Incremental method
				 * reduces the need for calibration of the servo movement while
				 * moving toward the target.
				 */
				incrementH = horizontalDestination - horizontalPosition;
				// you may need to reverse this based on your vertical servo installation
				incrementV = verticalPosition - verticalDestination; //use this one b/c camera upside down
				//incrementV = verticalDestination - verticalPosition;
				adjustServoPositions( incrementH, incrementV );  
				
				CameraDrive(horizontalDestination); // JDM
				
				ShowActivity ("** %s & %s found: Servo: x: %f  y: %f ** ", 
						td1.name, td2.name, horizontalDestination, verticalDestination);	
				
			} 
			else { //if (!staleImage) {  // new image, but didn't find two colors
				
				leftMotor->Set(0.0);
				rightMotor->Set(0.0);
				
				// adjust sine wave for panning based on last movement direction
				if(horizontalDestination > 0.0)	
					sinStart = PI/2.0; 
				else 
					sinStart = -PI/2.0; 

				/* pan to find color after a short wait to settle servos
				 * panning must start directly after panInit or timing will be off */				
				if (panIncrement == 3) {
					panInit(8.0);		// number of seconds for a pan
				}
				else if (panIncrement > 3) {					
					panForTarget(horizontalServo, sinStart);	
					
					/* Vertical action: In case the vertical servo is pointed off center,
					 * center the vertical after several loops searching */
					if (panIncrement == 20) { verticalServo->Set( 0.5 );	}
				}
				panIncrement++;		

				ShowActivity ("** %s and %s not found", td1.name, td2.name);
			}  // end if found color

			// sleep to keep loop at constant rate
			// this helps keep pan consistant
			// elapsed time can vary significantly due to debug printout
			currentTime = GetTime();			
			lastTime = currentTime;					
			if ( loopTime > ElapsedTime(lastTime) ) {
				Wait( loopTime - ElapsedTime(lastTime) );	// seconds
			}			
		}  // end while
	    
		leftMotor->Set(0.0);
		rightMotor->Set(0.0);
				
		DPRINTF(LOG_DEBUG, "end Autonomous");
		ShowActivity ("Autonomous end");
	}  // end autonomous

	void Neurologist::OperatorControl(void)  {
		//char funcName[]="OperatorControl";
		DPRINTF(LOG_DEBUG, "OperatorControl");
		
		GetWatchdog().SetEnabled(true);

		while ( IsOperatorControl() )
		{
			GetWatchdog().Feed();
							
			//ModeLogic();
			DriveCode(); 
			LiftCode();
			RollerCode();
			/*ShowActivity("leftMotorCmd = %f\t rightMotorCmd = %f\t Joy Y = %f\t Joy X = %f\t speed = %f\t turn = %f",
						  leftMotorCmd, rightMotorCmd, driveStick->GetY(), driveStick->GetX(), speed, turn );*/
		}  
	} // end operator control	
	
	
	void Neurologist::DriveCode() {
		//Set Motor Commands
			
		//mode logic for Calibration 
		//Start by turning on guarded cal_switch and abort calibration if the trigger is pressed  
		if(cal_switch_on)
			CalibrateTorqueLimits();			
		else{
			GetStickData();
			leftMotorCmd = speed + turn;
			rightMotorCmd = speed - turn;
		/*	if(speed > 0 && turn == 0 && sw_headingHold_on){
				turn = HeadingHold();
			}
			else
				headingRef = heading;
				
			TractionControl(); */
			SetMotorData();
		}
	}
	
	void Neurologist::SetMotorData(){
		leftMotor->Set(leftMotorCmd);
		rightMotor->Set(rightMotorCmd);  //right motor is reversed
	}
	
	void Neurologist::GetStickData(){
		float xt, yt;
		
		if(driveStick->GetX() > -deadzone  && driveStick->GetX() < deadzone) 
			xt = 0;
							
	    else if(driveStick->GetX() >= deadzone )
			xt = slope*driveStick->GetX() - intercept;
					
	    else
		    xt = slope*driveStick->GetX() + intercept;

		if(driveStick->GetY() > -deadzone  && driveStick->GetY() < deadzone )
			yt = 0;
							
		else if(driveStick->GetY() >= deadzone )
			yt = slope*driveStick->GetY() - intercept;
							
		else
			yt = slope*driveStick->GetY() + intercept;
				
		turn = xt;
		speed = yt;
		
		ShowActivity("xt = %f\t yt = %f", xt, yt);
		
	}
	
	void Neurologist::LiftCode() {
		static Relay::Value lastMovementValue;
		
		if(liftStick->GetRawButton(10) && liftStick->GetRawButton(11)){

			if(lastMovementValue == Relay::kForward){
				liftR->Set(Relay::kReverse);
				liftL->Set(Relay::kReverse);
				lastMovementValue = Relay::kReverse;
			}
			
			else if(lastMovementValue == Relay::kReverse){
				liftR->Set(Relay::kForward);
				liftL->Set(Relay::kForward);
				lastMovementValue = Relay::kForward;
			}		
		}
		
		if(liftStick->GetRawButton(10)){
			liftR->Set(Relay::kForward);
			liftL->Set(Relay::kForward);
			lastMovementValue = Relay::kForward;
		}
		
		else if(liftStick->GetRawButton(11)){
			liftR->Set(Relay::kReverse);
			liftL->Set(Relay::kReverse);
			lastMovementValue = Relay::kReverse;
		}
							
	    else{
			liftR->Set(Relay::kOff);
			liftL->Set(Relay::kOff);
	    }
		
		if(toplimitswitch->Get() == 0 && lastMovementValue == Relay::kForward){
			liftR->Set(Relay::kOff);
			liftL->Set(Relay::kOff);
		}
		
		if(bottomlimitswitch->Get() == 0 && lastMovementValue == Relay::kReverse){
			liftR->Set(Relay::kOff);
			liftL->Set(Relay::kOff);
		}
	}
	
	void Neurologist::RollerCode() {
		static float rollerlast;
		
		if (liftStick->GetRawButton(6) && liftStick-> GetRawButton(7)) {  //if person accidentally keeps finger on old button while pressing 
			rollerL->Set(-rollerlast);                                     //new button, get reverse of old value
		}
		else if (liftStick->GetRawButton(6)) {
			rollerL->Set(1.0);
			rollerlast = 1;
		}
		
		else if(liftStick->GetRawButton(7)){
			rollerL->Set(-1.0);
			rollerlast = -1;
		}
		
		else {
			rollerL->Set(0.0);
			rollerlast = 0;
		}
		
		rollerR->Set(-rollerL->Get());
	}
	
	/****************************************************************************************************************************
	 * TODO make constant params 																								*
	 * sw_use_ref_wheel	 , sw_torqlim_on  , tq_limit_ratio , l_cmd_limit_pos, l_cmd_lim_neg, rightCmd_lim_pos,rightCmd_lim_neg 	*
	 * sw_compensate_battery 																									*
	 * This routine determines if the left and right wheels are slipping based upon reference wheels speeds						*
	 * If the wheels are slipping, the torque limits are reduced by tq_limit_ratio.. ie cmd_limit= cmd_limit*tq_limit_ratio		*
	 * Then the torque limit functions are applied to the motors.																*
	 * 																															*
	 * Check to see if the average linear velocity of the wheels with motors													*
	 * attached is more than the linear velocity of the robot itself.           CDS changed less to  "more"						*
	 * If this is the case, limit the amount of torque allowed to the motors.													*
	 ****************************************************************************************************************************/
	 void Neurologist::TractionControl (){
		
		static float l_ratio= 0; 
		static float r_ratio= 0;
		static float leftCmd= 0;
		static float rightCmd = 0;
		 
		//capture the input command 
		leftCmd = leftMotorCmd ;
		rightCmd = rightMotorCmd;
	  
		//left wheel slipping?
				  
		v_l_slip = fabs(v_l) - fabs(v_lref) ;
		if(v_l_slip > tol && sw_use_ref_wheel){
			l_ratio= tq_limit_ratio;
		}
		else{
			l_ratio = 1;
		}
		  
		//right wheel slipping?
		v_r_slip = fabs(fabs(v_r) - fabs(v_rref));
		if(v_r_slip > tol  && sw_use_ref_wheel){
			r_ratio= tq_limit_ratio;
		}
	  	else{
	  		r_ratio = 1;
	  	}
				
		if(sw_torqlim_on){
			leftCmd  = TorqueLimiter(leftCmd, l_rate_rps/maxWheelRate, leftCmd_limit_pos, leftCmd_limit_neg,  l_ratio);
			rightCmd = TorqueLimiter(rightCmd,r_rate_rps/maxWheelRate, rightCmd_limit_pos, rightCmd_limit_neg,  r_ratio);
		}
		//Compensate command for battery voltage variations 
		if(sw_compensate_battery){
			if(v_battery < 5) v_battery = 5; // limit the ammount of correction...just in case the battery voltage goes to zero 
				battery_factor = 12/v_battery ; 
				leftCmd = leftCmd*battery_factor; 
				rightCmd = rightCmd*battery_factor;
		}// end battery compensation test
	    
		leftMotorCmd = leftCmd;
		rightMotorCmd = rightCmd;
	}
	 
	/************************************************************************************
	 * This function is used when the Torque Calibration Guarded switch is activated.	*
	 * 																					*
	 * Positive torque limits are determined if driveStick is pushed > 50% positive.	*
	 * Negative torque limits are determined if driveStick is pulled < 50% negative.	*
	 * 																					*
	 * The driveStick must be held until the calibration is completed. If released		*
	 * before the cal_done = TRUE is achieved and error will be printed and the			*
	 * calibration is aborted.															*
	 * 																					*
	 * After cal_done = TRUE, the function is reset by centering the driveStick.		*
	 * The stick is then pushed in the opposite direction and the opposite sign limit	*
	 * is determined. Once both limits are determined, the robot motor commands will	*
	 * be zeroed and stay that way until the Torque Calibration Guarded switch is		*
	 * deactivated.																		*
	 * 																					*
	 * When the driveStick activates the calibration sequence the normal drive			*
	 * functions are bypassed and a full step command is input to the motors with		*
	 * torque command limits set to zero. These command limits are gradually			*
	 * increased until a slip condition is detected by either wheel speed sensors at	*
	 * which time, the limit is frozen and held ready for saving to non_volitile		*
	 * memory and the motor command is zeroed. The remaining motor torque limit			*
	 * continues to increase until its limit is determined. When both sides have		*
	 * determined their torque limit,cal_done = TRUE and the limits are written to		*
	 * non-volitile memory.  The calibration function is reset by centering the			*
	 * driveStick.																		*
	 * 																					*
	 **********IMPORTANT*****************************************************************
	 ************************************************************************************
	 * First secure the robot so it can't move in the forward or aft direction or turn	*
	 * if one motor is on and the other is not. 										*										*
	 ************************************************************************************/
	void Neurologist::CalibrateTorqueLimits() {
				
		static float limit_rate = .01; //rate at which the command limit is increased ....move this to constructor
		static bool cal_started= false;
		static bool cal_done = false;
		static bool left_done = false;
		static bool right_done = false;
		static float leftCmd;
		static float rightCmd;
			
		v_l_slip = fabs((v_l) - fabs(v_lref));
		v_r_slip = fabs((v_r) - fabs(v_rref));
		
		//If we want to compensate command limits for battery voltage variations, read the voltage here.
		if(sw_compensate_battery){
			v_battery = ds->GetBatteryVoltage();
			if(v_battery < 5){
				v_battery = 5;
			}// limit the ammount of correction...just in case the battery voltage goes to zero 
		}
			
		//If driveStick forward then calibrate right and left positive limits
		if(driveStick->GetY()>.5){
			if(cal_started == false){
				cal_started = true;
				printf("Torque Cal:   Starting positive limit calibration \n");
				leftCmd=1;
				rightCmd=1;
			}//Give motors full foward command 
			if(!left_done){
				if(v_l_slip < tol){
					leftCmd_limit_pos = leftCmd_limit_pos + limit_rate;
				}//if not slipping increase limit
				else {
					leftCmd=0.;
					left_done = true;
					printf("positive leftCmd_limit = %.4f \n" , leftCmd_limit_pos);
				}
			}
			if(!right_done){
				if(v_r_slip < tol ){
					rightCmd_limit_pos = rightCmd_limit_pos + limit_rate;
				}//if not slipping increase limit
				else{
					rightCmd=0;
					right_done = true;
					printf("positive rightCmd_limit = %.4f \n", rightCmd_limit_pos);
				}
			}
			if(left_done && right_done && !cal_done){
				printf("Positive limits completed, exiting Calibration routine\n");
				cal_done = true;

				limitFile = fopen("robotData.txt", "r");
				fread(robotData, 4, sizeof(float), limitFile);
				
				robotData->leftCmd_limit_pos = leftCmd_limit_pos*v_battery/12;  //save nv limit normalized to 12 v
				robotData->rightCmd_limit_pos = leftCmd_limit_pos*v_battery/12;  //save nv limit normalized to 12 v 		

				fwrite(robotData, 4, sizeof(float), limitFile);
				
				fclose(limitFile);
			}
			leftCmd = TorqueLimiter(leftCmd ,l_rate_rps/maxWheelRate, leftCmd_limit_pos, 1, 1);
			rightCmd = TorqueLimiter(rightCmd,r_rate_rps/maxWheelRate, rightCmd_limit_pos, 1, 1);
		}
		// else if driveStick is reversed then calibrate right and left negative limits
		else if(driveStick->GetY() <-0.5 ) {
			if(cal_started == false){
				cal_started = true;
				printf("Torque Cal:   Starting negative limit calibration \n");
				leftCmd=-1; rightCmd=-1;//Give motors full reverse command
			}
			if(!left_done) {
				if(v_l_slip < tol){
					leftCmd_limit_neg = leftCmd_limit_neg + limit_rate;
				}
				else{
					leftCmd=0;
					left_done = true;
					printf("negative leftCmd_limit = %.4f \n", leftCmd_limit_neg);
				}
			}
			if(!right_done) {
				if(v_r_slip < tol ){
					rightCmd_limit_neg = rightCmd_limit_neg + limit_rate;
				}
				else {
					rightCmd=0;
					right_done = true;
					printf("negative rightCmd_limit = %.4f \n", rightCmd_limit_neg);
				}
			}
			if(left_done && right_done && !cal_done) {
				printf("negative limits completed, exiting Calibration routine \n");
				cal_done = true;

				limitFile = fopen("robotData.txt", "r");
				fread(robotData, 4, sizeof(float), limitFile);
				
				robotData->leftCmd_limit_neg = leftCmd_limit_neg*v_battery/12;  //save nv limit normalized to 12 v
				robotData->rightCmd_limit_neg = leftCmd_limit_neg*v_battery/12;  //save nv limit normalized to 12 v
				
				fwrite(robotData, 4, sizeof(float), limitFile);		
				
				fclose(limitFile);
			}
			leftCmd = TorqueLimiter(leftCmd ,l_rate_rps/maxWheelRate, 1, leftCmd_limit_neg, 1);
			rightCmd = TorqueLimiter(rightCmd,r_rate_rps/maxWheelRate, 1, rightCmd_limit_neg,1);
		}
		
		//If stick is released before calibration is finished, print out warning that limits were not computed
		else {
			if(cal_started && !cal_done){
				printf("Stick released before Calibraion completed...no limits computed \n");
			}
			cal_started = FALSE; //reset for new cal
			cal_done=FALSE;//reset for new cal
			left_done = FALSE;//reset for new cal
			right_done= FALSE;//reset for new cal
		}
		
		// make sure that motors are off until we move the drivestick
		if(!cal_started){
			leftCmd = 0;
			rightCmd = 0;
		}
		leftMotor->Set(leftCmd);
		rightMotor->Set(rightCmd);
		Wait(.2);//put a delay here to allow motors time to respond
	}

	//CDS here is a prototype for a torque limiter function
	float Neurologist::TorqueLimiter(float input_cmd, float motor_rate_norm , float cmd_limit_pos, float cmd_limit_neg, float ratio){
		// This function uses motor date independent of a reference wheel to limit torque
		// input_cmd = motor command [-1,1] 
		// motor_rate_norm = normalized motor rate [-1,1]   This is the motor speed / max motor speed(corrected for voltage variation)
		//cmd_limit_pos  = motor torque limit (normalized units) in forward direction
		//cmd_limit_neg  = motor torque limit (normalized units) in aft direction   Note; this is specified as a positive number even though it limits neg values
		//limited_cmd  = the value of the command that is restriced to keep torque below the limit

		static float limited_cmd = 0;
		
		cmd_limit_pos = cmd_limit_pos*ratio;
		cmd_limit_neg = cmd_limit_neg*ratio;
		if( (input_cmd-motor_rate_norm) > cmd_limit_pos ){
			limited_cmd = motor_rate_norm + cmd_limit_pos  ;
		}
		else  if( (input_cmd-motor_rate_norm) < -cmd_limit_neg ){
			limited_cmd = motor_rate_norm - cmd_limit_neg  ;
		}
		else{
			limited_cmd = input_cmd;
		}
		return limited_cmd ;
	}//end TorqueLimiter routine

	void Neurologist::ModeLogic(){
		
		while(driveStick->GetRawButton(8) && driveStick->GetRawButton(9) && !cal_switch_on){
			Wait(1.0);
			cal_switch_on = driveStick->GetRawButton(8) && driveStick->GetRawButton(9);
		}
		while(driveStick->GetRawButton(6) && driveStick->GetRawButton(11) && !sw_torqlim_on){
			Wait(1.0);
			if(driveStick->GetRawButton(6) && driveStick->GetRawButton(11)){
				sw_torqlim_on = true;				
			}
		}
		while(driveStick->GetRawButton(7) && driveStick->GetRawButton(10)&& sw_torqlim_on){
			Wait(1.0);
			if(driveStick->GetRawButton(7) && driveStick->GetRawButton(10)){
				sw_torqlim_on = false;
			}
		}	
	}

	float Neurologist::HeadingHold(){
		//define Kp as heading error gain and Kd as heading rate gain
		//heading and heading rate are in dergrees and degrees per second
		return Kp*(headingRef - heading) - (Kd * headingRate);
	}



// entry point FRC_UserProgram_StartupLibraryInit
START_ROBOT_CLASS(Neurologist);
